/*
 * Copyright (c) 2022-2023 Termony Co., Ltd. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of
 *    conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list
 *    of conditions and the following disclaimer in the documentation and/or other materials
 *    provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used
 *    to endorse or promote products derived from this software without specific prior written
 *    permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef ASSEMBLY
#define ASSEMBLY 1
#endif

#include "los_arch_assembler.h"
#include "los_arch_marcos.h"
#include "target_config.h"

FILE_BEGIN()

    /* Variables and functions. */
    EXPORT_LABEL(HalStartToRun)
    EXPORT_LABEL(HalTaskSwitch)
    EXPORT_LABEL(HalTaskSwitchFromISR)

    IMPORT_LABEL(g_losTask)
    IMPORT_LABEL(g_intCount)
    IMPORT_LABEL(g_taskYieldRequired)
    IMPORT_LABEL(g_taskHasFPUContext)
    IMPORT_LABEL(OsSchedTaskSwitch)
    IMPORT_LABEL(OsSignalTaskContextRestore)


/* 保存任务上下文. */
MACRO_DEF(SAVE_TASK_CONTEXT)
    /* 切换到使用EL0堆栈指针. */
    MSR     SPSEL, #0

    /* 保存通用寄存器X0 ~ X30和SP. */
    PUSH_REG_ALL_COMMON SP
    /* 获取寄存器ELR和SPSR并保存. */
    GET_REG_ELR_SPSR X0 X2 X3
    STP     X2, X3, [SP, #-0x10]!

#if (LOSCFG_PLATFORM_FPU_ENABLE > 0)
    /* 保存浮点寄存器V0 ~ V31. */
    PUSH_VREG_ALL_128B SP
    /* 保存浮点寄存器FPCR及FPSR. */
    MRS     X1, FPSR /* X1 = FPSR */
    MRS     X0, FPCR /* X0 = FPCR */
    STP     X0, X1, [SP, #-0x10]!
#endif /* LOSCFG_PLATFORM_FPU_ENABLE */

    /* 修改任务栈指向SP. */
    LDR     X0, =g_losTask
    LDR     X1, [X0]
    MOV     X2, SP
    STR     X2, [X1]

    /* 切换到使用ELx堆栈指针. */
    MSR     SPSEL, #1
MACRO_END()


/* 恢复任务上下文. */
MACRO_DEF(RESTORE_TASK_CONTEXT)
    /* 切换到使用EL0堆栈指针. */
    MSR     SPSEL, #0

    /* 修改SP指向任务栈. */
    LDR     X0, =g_losTask
    LDR     X1, [X0]
    LDR     X2, [X1]
    MOV     SP, X2

#if (LOSCFG_PLATFORM_FPU_ENABLE > 0)
    /* 恢复浮点寄存器FPCR及FPSR. */
    LDP     X0, X1, [SP], #0x10
    MSR     FPSR, X1 /* FPSR = X1 */
    MSR     FPCR, X0 /* FPCR = X0 */

    /* 恢复浮点寄存器V0 ~ V31. */
    POP_VREG_ALL_128B SP
#endif /* LOSCFG_PLATFORM_FPU_ENABLE */

    /* 恢复寄存器ELR和SPSR并设置. */
    LDP     X2, X3, [SP], #0x10
    SET_REG_ELR_SPSR X0 X2 X3
    /* 恢复通用寄存器X0 ~ X30. */
    POP_REG_ALL_COMMON SP

    /* 切换到使用ELx堆栈指针. */
    MSR     SPSEL, #1
    ERET
MACRO_END()


/* 恢复信号上下文. */
MACRO_DEF(SIGNAL_CONTEXT_RESTORE reg)
    /* 切换到使用EL0堆栈指针. */
    MSR     SPSEL, #0

    /* 修改SP指向信号栈. */
    MOV     SP, \reg

    #if (LOSCFG_PLATFORM_FPU_ENABLE > 0)
    /* 恢复浮点寄存器FPCR及FPSR. */
    LDP     X0, X1, [SP], #0x10
    MSR     FPSR, X1 /* FPSR = X1 */
    MSR     FPCR, X0 /* FPCR = X0 */

    /* 恢复浮点寄存器V0 ~ V31. */
    POP_VREG_ALL_128B SP
#endif /* LOSCFG_PLATFORM_FPU_ENABLE */

    /* 恢复寄存器ELR和SPSR并设置. */
    LDP     X2, X3, [SP], #0x10
    SET_REG_ELR_SPSR X0 X2 X3
    /* 恢复通用寄存器X0 ~ X30. */
    POP_REG_ALL_COMMON SP

    /* 切换到使用ELx堆栈指针. */
    MSR     SPSEL, #1
    ERET
MACRO_END()


FUNC_DEF(HalStartToRun)
    /* 让运行任务TCB指向新任务TCB, 用于切换任务. */
    LDR     X0, =g_losTask
    LDR     X1, [X0, #8]
    STR     X1, [X0]

    /* 恢复运行任务的上下文. */
    RESTORE_TASK_CONTEXT
    FUNC_END()


FUNC_DEF(HalTaskSwitch)
    /* 保存当前任务的上下文. */
    SAVE_TASK_CONTEXT

    /* 调用LiteOS-M的信号切换接口. */
    BL      OsSignalTaskContextRestore
    CMP     X0, #0
    BEQ     TASK_SWITCH_CHECK
    /* 恢复信号上下文. */
    SIGNAL_CONTEXT_RESTORE X0

TASK_SWITCH_CHECK:
    /* 调用LiteOS-M的任务切换接口. */
    BL      OsSchedTaskSwitch
    CMP     X0, #0
    BEQ     1f

    /* 让运行任务TCB指向新任务TCB, 用于切换任务. */
    LDR     X0, =g_losTask
    LDR     X1, [X0, #8]
    STR     X1, [X0]

1:
    /* 恢复运行任务的上下文. */
    RESTORE_TASK_CONTEXT
    FUNC_END()


FUNC_DEF(HalTaskSwitchFromISR)
    /* 保存X0 ~ X3寄存器. */
    STP     X0, X1, [SP, #-0x10]!
    STP     X2, X3, [SP, #-0x10]!
    /* 获取寄存器ELR和SPSR并保存. */
    GET_REG_ELR_SPSR X0 X2 X3
    STP     X2, X3, [SP, #-0x10]!

    /* 如果中断嵌套计数不为0, 则不执行上下文切换. */
    LDR     X1, =g_intCount
    LDR     X0, [X1]
    CMP     X0, #0
    BNE     EXIT_WITHOUT_SWITCH

    /* 判断中断是否请求上下文切换. */
    LDR     X1, =g_taskYieldRequired
    LDR     X0, [X1]
    CMP     X0, #0
    BEQ     EXIT_WITHOUT_SWITCH

    /* 清除上下文切换请求标识. */
    MOV     X0, #0
    STR     X0, [X1]

    /* 恢复寄存器ELR和SPSR并设置. */
    LDP     X2, X3, [SP], #0x10
    SET_REG_ELR_SPSR X0 X2 X3
    /* 恢复X0 ~ X3寄存器. */
    LDP     X2, X3, [SP], #0x10
    LDP     X0, X1, [SP], #0x10

    /* 切换任务上下文. */
    B       HalTaskSwitch

EXIT_WITHOUT_SWITCH:
    /* 恢复寄存器ELR和SPSR并设置. */
    LDP     X2, X3, [SP], #0x10
    SET_REG_ELR_SPSR X0 X2 X3
    /* 恢复X0 ~ X3寄存器. */
    LDP     X2, X3, [SP], #0x10
    LDP     X0, X1, [SP], #0x10
    ERET
    FUNC_END()

FILE_END()
